
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;



/**
 *
 */
public class Climber extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    Relay leftClimberMotor;
    Relay rightClimberMotor;
    Solenoid deployLeftSolenoid;
    Solenoid retractLeftSolenoid;
    Solenoid deployRightSolenoid;
    Solenoid retractRightSolenoid;
    
    public String climberPosition = "";
    public boolean climberDeployed;
        
    public Climber(){
        super("Climber");        
        try {
            leftClimberMotor = new Relay(RobotMap.CLIMBER_LEFT_RELAY, Relay.Direction.kBoth);    
            rightClimberMotor = new Relay(RobotMap.CLIMBER_RIGHT_RELAY, Relay.Direction.kBoth);
            deployLeftSolenoid = new Solenoid(RobotMap.LEFT_DEPLOY_SOLENOID);
            retractLeftSolenoid = new Solenoid(RobotMap.LEFT_RETRACT_SOLENOID);
            deployRightSolenoid = new Solenoid(RobotMap.RIGHT_DEPLOY_SOLENOID);
            retractRightSolenoid = new Solenoid(RobotMap.RIGHT_RETRACT_SOLENOID);
        }
        catch (Exception e){
            e.printStackTrace();
        }
    }
    public void initDefaultCommand() {
    }    
    
    public void deployClimber() {
        deployLeftSolenoid.set(true);
        retractLeftSolenoid.set(false);
        deployRightSolenoid.set(true);
        retractRightSolenoid.set(false);
        climberDeployed = true;
    }
    
    public void retractClimber() {
        retractLeftSolenoid.set(true);
        deployLeftSolenoid.set(false);
        retractRightSolenoid.set(true);
        deployRightSolenoid.set(false);
        climberDeployed = false;
    }
    
    public boolean isClimberDeployed() {
        return climberDeployed;
    }
    
    public void ascend() {
        leftClimberMotor.set(Relay.Value.kForward);
        rightClimberMotor.set(Relay.Value.kForward);
        climberPosition = "ascend";
    }   
    
    public void descend() {
        leftClimberMotor.set(Relay.Value.kReverse);
        rightClimberMotor.set(Relay.Value.kReverse);
        climberPosition = "descend";
    }   
    
    public void stop(){
        leftClimberMotor.set(Relay.Value.kOff);
        rightClimberMotor.set(Relay.Value.kOff);
        climberPosition = "stop";
    }   
    
    public String climberPosition() {
        return climberPosition;
    }
 }